#!/usr/bin/env python3
import numpy as np

class State:

    def __init__(self, pos: np.ndarray, time: int, g_score: int, h_score: int, start, goal):
        self.pos = pos
        self.time = time
        self.g_score = g_score
        # self.f_score = g_score + h_score
        f = g_score + h_score
        # tie-breaker 的方式之一，加一点“飘柔”
        # self.f_score = f + f*0.001
        # tie-breaker 方式之二。加上叉积，使路线倾向于起点到终点的直线
        dx1 = pos[0] - goal[0]
        dy1 = pos[1] - goal[1]
        dx2 = start[0] - goal[0]
        dy2 = start[1] - goal[1]
        cross = abs(dx1*dy2 - dx2*dy1)
        self.f_score = f + cross*0.001



    def __hash__(self) -> int:
        concat = str(self.pos[0]) + str(self.pos[1]) + '0' + str(self.time)
        return int(concat)

    def pos_equal_to(self, pos: np.ndarray) -> bool:
        return np.array_equal(self.pos, pos)

    def __lt__(self, other: 'State') -> bool:
        return self.f_score < other.f_score

    def __eq__(self, other: 'State') -> bool:
        return self.__hash__() == other.__hash__()

    def __str__(self):
        return 'State(pos=[' + str(self.pos[0]) + ', ' + str(self.pos[1]) + '], ' \
               + 'time=' + str(self.time) + ', fscore=' + str(self.f_score) + ')'

    def __repr__(self):
        return self.__str__()

if __name__ == '__main__':
    
    def h(start, goal):
        return int(np.linalg.norm(start-goal, 1))  # L1 norm
    
    def snap_to_grid(position):
        grid_size = 2
        miny = 1
        minx = 1
        g =np.array([[[2, 2],
                    [4, 2]],
                   [[2, 4],
                    [4, 4]]])
        i = (position[1] - miny) // grid_size
        j = (position[0] - minx) // grid_size
        return g[i][j]
    
    start = (2,2)
    goal = (2,4)    
    start = snap_to_grid(np.array(start))
    goal = snap_to_grid(np.array(goal))
    s = State(start, 0, 0, h(start, goal))
    g = State(goalg, 0, 0, h(start, goal))